#include <boost\heap\fibonacci_heap.hpp>
#include <opencv2\opencv.hpp>
#include <vector>
#include <pcl\point_cloud.h>
#include <pcl\point_types.h>



class Surface{

public:
	typedef struct polarCoordinate{
		float r;
		float theta;

		polarCoordinate(float r, float theta){
			this->r = r;
			this->theta = theta;
		}
	};

	struct comparePolarCoordinate {
		bool operator() (const polarCoordinate& lhs, const polarCoordinate& rhs) const{
			return lhs.theta > rhs.theta;
		}
	};

	Surface(int id);
	~Surface();

	
	void accumulatePoints(float x, float y, float z, bool is_boundary);
	void compressSurface();
	void mergeSurface(Surface* other);
	int getBoundarySize() const;
	int getPointsSize() const;
	int getSurfaceID() const;
	bool comparePlaneParameters(Surface* other) const;
	std::vector<cv::Vec3d> getReconstructedPointsVector() const;
	pcl::PointCloud<pcl::PointXYZ>* getPointCloudRepresentation();
	
	


private:	
	enum PLANE_DIRECTION{X, Y, Z};
	
	double basis_x;
	double basis_y;	
	float nx, ny, nz, d;
	int x_boundary, y_boundary;
	int surfaceID;
	int size;
	int p_face;

	cv::Mat_<float> boundary_temp;		//row-major matrix
	cv::Mat_<float> planeApproximation;	//row-major matrix
	cv::Mat_<float> plane_center;		//vector
	boost::heap::fibonacci_heap<polarCoordinate, boost::heap::compare<comparePolarCoordinate>> boundary;

	void accumulateBoundary(cv::Mat_<float> point);
	polarCoordinate getPolarCoordinate(double x, double y) const;
	polarCoordinate getPolarCoordinate(cv::Vec2d in) const;
	cv::Vec2d getCartesianCoordinate(polarCoordinate in) const;
	cv::Vec3d get3DCartesianCoordinate(polarCoordinate in) const;
	void determineDominentFactor();
	void setBasis();
	void setRelativeCoordinateSystem();
	void changePole(cv::Mat_<float> new_pole);
	void storeBoundary();
	void getPlaneParameters();


	bool isCenterOnPlane();
	
};